#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "hi_io.h"
#include "hi_time.h"
#include "iot_watchdog.h"
#include "robot_control.h"
#include "iot_errno.h"
#include "hi_pwm.h"
#include "hi_timer.h"
#include "iot_pwm.h"
#include <time.h>
#include <stdio.h>

//左右两轮电机各由一个L9110S驱动
//GPOI0和GPIO1控制左轮,GPIO9和GPIO10控制右轮。通过输入GPIO的电平高低控制车轮正转/反转/停止/刹车。
#define GPIO0 0
#define GPIO1 1
#define GPIO9 9
#define GPIO10 10

//查阅机器人板原理图可知
//左边的红外传感器通过GPIO11与3861芯片连接
//右边的红外传感器通过GPIO12与3861芯片连接
#define GPIO11 11
#define GPIO12 12

#define GPIO_FUNC 0
#define car_speed_left 0
#define car_speed_right 0

extern unsigned char g_car_status;   
IotGpioValue io_status_left;
IotGpioValue io_status_right;

// 终点线相关模块
static time_t g_last_destination_time = 0;  // 上次检测到终点线的时间
static const int g_destination_timeout = 7;  // 终点线超时时间（单位：秒）
static int g_destination_counter = 0;  // 终点线检测计数
#define DESTINATION_COUNTER_MAX 3

// #include "speed.h"
extern float forward_speed;
extern float GetDistance();
// 新增：避障相关变量
#define DISTANCE_BETWEEN_CAR_AND_OBSTACLE 20.0  // 20cm障碍物检测距离忍受
static int g_obstacle_detected = 0;  // 障碍物检测标志
// 速度相关变量
#define MAX_SPEED_CHANGE 100
float llast_distance = 0.0;
float last_distance = 0.0;
float distance = 0.0;

// 获取小车方向
typedef enum CarDirection {
    DIR_FORWARD = 1,
    DIR_BACKWARD = 2,
    DIR_LEFT = 3,
    DIR_RIGHT = 4,
    DIR_STOP = 5,
    DIR_BIG_LEFT = 6,
    DIR_BIG_RIGHT = 7,
}CarDir;
extern CarDir car_dir_index;
extern void car_run(CarDir);
CarDir car_dir_index_last;

#define IS_RETACKING 1
#define ISNOT_RETACKING 0
int is_retracking = ISNOT_RETACKING;
// 回转时间（单位：毫秒------------------）
#define MAX_RETRACKING_TIME_COUNT 4
int retracking_time_count = MAX_RETRACKING_TIME_COUNT;
void is_retracking_set(int status){
    is_retracking = status;
    retracking_time_count = MAX_RETRACKING_TIME_COUNT;
}

//获取红外传感器的值，调整电机的状态
void timer1_callback(unsigned int arg)
{
    IotGpioValue io_status;

    // 如果检测到障碍物，直接返回，不执行循迹逻辑
    if (g_obstacle_detected) {
        return;
    }

    if(is_retracking == IS_RETACKING){
        retracking_time_count--;
        printf("retracking_time_count: %d\n", retracking_time_count);
        if(retracking_time_count <= 0){
            is_retracking_set(ISNOT_RETACKING);
            car_dir_index = DIR_STOP;
            printf("run stop\n");
        }
        return;
    }

    IoTGpioGetInputVal(GPIO11,&io_status_left); //获取GPIO11引脚的输入电平值
    IoTGpioGetInputVal(GPIO12,&io_status_right);//获取GPIO12引脚的输入电平值

    // 如果正在右转，只关注左边的传感器，当左边传感器检测到黑线时，回调一段时间后恢复直行
    if(car_dir_index == DIR_BIG_RIGHT){
        if(io_status_left == IOT_GPIO_VALUE0){
            car_dir_index = DIR_BIG_LEFT; 
            is_retracking_set(IS_RETACKING);
        }
        return;
    }

    // 如果正在左转，只关注右边
    if(car_dir_index == DIR_BIG_LEFT){
        if(io_status_right == IOT_GPIO_VALUE0){
            car_dir_index = DIR_BIG_RIGHT;
            is_retracking_set(IS_RETACKING);
        }
        return;
    }

    // 如果没有偏离，直行
    if(io_status_right != IOT_GPIO_VALUE0 && io_status_left != IOT_GPIO_VALUE0){
        car_dir_index = DIR_FORWARD;
    }
    
    //小车往左偏，则需要向右修正方向(右转)
    //如果GPIO12输入低电平（右边的红外传感器检测到黑色）并且GPIO11输入高电平（左边的红外传感器未检测到黑色）
    if(io_status_right == IOT_GPIO_VALUE0 && io_status_left != IOT_GPIO_VALUE0)
    {   
        car_dir_index = DIR_BIG_RIGHT;
    } 

    //小车往右偏，则需要向左修正方向（左转）
    //如果GPIO12输入高电平（右边的红外传感器未检测到黑色）并且GPIO11输入低电平（左边的红外传感器检测到黑色）
    if(io_status_right != IOT_GPIO_VALUE0 && io_status_left == IOT_GPIO_VALUE0)
    {
        car_dir_index = DIR_BIG_LEFT;
    }
    //如果两个红外传感器都检测到黑色，则小车停止
    if(io_status_right == IOT_GPIO_VALUE0 && io_status_left == IOT_GPIO_VALUE0)
    {
        //终点计数
        time_t current_time = time(NULL);
        if (current_time - g_last_destination_time >= g_destination_timeout) {
            g_destination_counter++;
            printf("Destination reached! Counter: %d\n", g_destination_counter);
            if(g_destination_counter >= DESTINATION_COUNTER_MAX){
                g_car_status = 0;
                forward_speed = 0;
            }
            else{
                car_dir_index = DIR_FORWARD;
            }
        }
        g_last_destination_time = current_time;
        forward_speed = 0;
    }
}

// 检测障碍物距离和计算实时速度
void timer2_callback(void){
    // 获取前方距离
    // printf("distance:%f last_distance:%f\n",distance,last_distance);
    distance = GetDistance(); // 先获取距离
    hi_udelay(100000);
    if(car_dir_index == DIR_FORWARD){
        // printf("distance:%f last_distance:%f llast_distance:%f\n",distance,last_distance,llast_distance);
        float now_speed = (llast_distance - last_distance) * 1;
        // 添加速度范围检查
        if (now_speed > 99) {
            now_speed = 99;
        } else if (now_speed < -99) {
            now_speed = -99;
        }else{}
        // printf("now_speed:%f forward_speed:%f\n",now_speed,forward_speed);
        forward_speed = now_speed;
        // printf("now_speed:%f forward_speed:%f\n",now_speed,forward_speed);
    }
    if (car_dir_index == DIR_STOP) {
        forward_speed = 0.0;
    }
    llast_distance = last_distance;
    last_distance = distance; // 更新 last_distance
    // 检查是否有障碍物
    if (distance < DISTANCE_BETWEEN_CAR_AND_OBSTACLE) {
        if (!g_obstacle_detected) {
            printf("🚨 Obstacle detected! Distance: %.2f cm\n", distance);
            g_obstacle_detected = 1;
            // 立即停止小车
            car_dir_index_last = car_dir_index;
            car_dir_index = DIR_STOP;
            forward_speed = 0;
        }
    } else {
        if (g_obstacle_detected) {
            printf("✅ Obstacle cleared! Distance: %.2f cm, resuming trace\n", distance);
            g_obstacle_detected = 0;
            car_dir_index = car_dir_index_last;
        }
    }
}

void trace_module(void)
{  
    regress_middle();
    g_destination_counter=0;
    unsigned int timer_id1;
    hi_timer_create(&timer_id1);
    unsigned int timer_id2;
    hi_timer_create(&timer_id2);
    unsigned int timer_id3;
    hi_timer_create(&timer_id3);

    //启动系统周期定时器用来按照预定的时间间隔1ms触发timer1_callback任务的执行
    hi_timer_start(timer_id1, HI_TIMER_TYPE_PERIOD, 1, timer1_callback, 0);
    // hi_timer_start(timer_id2, HI_TIMER_TYPE_PERIOD, 200, timer2_callback, 0);

    // gpio_control(GPIO0, IOT_GPIO_VALUE1);
    // gpio_control(GPIO1, g_car_speed_left);
    // gpio_control(GPIO9, IOT_GPIO_VALUE1);
    // gpio_control(GPIO10, g_car_speed_right);
    car_dir_index = DIR_FORWARD;
    
    while (1) { 
        // gpio_control(GPIO1, g_car_speed_left);
        // gpio_control(GPIO10, g_car_speed_right);
        car_run(car_dir_index);

        hi_udelay(20);

        if (g_car_status != CAR_TRACE_STATUS) {
            break;
        }
    }
    hi_timer_delete(timer_id1);
    hi_timer_delete(timer_id2);
    hi_timer_delete(timer_id3);
}